////////////////////////////////////
// version 0.7 de Roby
////////////////////////////////////
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// globales
unsigned long _rapport_turret_last = 0;
unsigned long _rapport_body_last = 0;
unsigned long _rapport_sensors_last = 0;
unsigned long _setting_rapport_stepping = 1;
unsigned long _setting_rapport_turret_interval = 1000/8;
unsigned long _setting_rapport_body_interval = 1000/8;
unsigned long _setting_rapport_sensors_interval = 1000/8;
boolean _send_status = false;
boolean _enable_debug = false;
boolean _send_invalid_cmd = true;

// LCDs
LiquidCrystal_I2C lcd_body(0x27,16,2); // set the LCD address to 0x27 for a 16 chars and 2 line display

// motors
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

// servos of turret
Servo servo_turret_low, servo_turret_high;

// IF Range Switch
const int switch_body_front_left = 10;
const int switch_body_front_right = 9;
const int switch_body_back_left = 11;
const int switch_body_back_right = 8;

// IF Range sensors
const int infra_range_turret = 2;
const int infra_range_body_front = 1;

// US Range sensors
const int sonic_range_turret = 3;

int ibound(int iMin, int iMax, int iVal)
{
  if(iVal<iMin) return iMin;
  else if(iVal>iMax) return iMax;
  return iVal;
}

float body_compass_bearing=0.0f;
float body_compass_pitch=0.0f;
float body_compass_roll=0.0f;

///////////////
// motor
void motor_stop(void)
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
}
void motor_action(const char a, const char b, const boolean a2, const boolean b2)
{
  analogWrite (E1,a);
  digitalWrite(M1,a2?HIGH:LOW);
  analogWrite (E2,b);
  digitalWrite(M2,b2?HIGH:LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}

//////////////
// turret
const int turret_low_min = 10, turret_low_mid = 80, turret_low_max = 170;
const int turret_high_min = 30, turret_high_mid = 85, turret_high_max = 105;
int current_turret_position_low = turret_low_mid;
int current_turret_position_high = turret_high_mid;

void reset_turret_position(const int dlay=50)
{
  current_turret_position_low = turret_low_mid;
  current_turret_position_high = turret_high_mid;
  servo_turret_low.write(turret_low_mid); 
  servo_turret_high.write(turret_high_mid);
  delay(dlay);    
}
void set_turret_position(const int low, const int high)
{
  int set_low = ibound(turret_low_min, turret_low_max, low);
  int set_high = ibound(turret_high_min, turret_high_max, high);

  // calcul delay from old position to new position
  int ml = abs(current_turret_position_low-set_low);
  int mh = abs(current_turret_position_high-set_high);
  long dlay = (ml+mh)*2.5f;

  servo_turret_low.write(set_low); 
  servo_turret_high.write(set_high);
  current_turret_position_low = set_low;
  current_turret_position_high = set_high;
  
  delay(dlay);
}
void acc_turret_position(const int low, const int high)
{
  set_turret_position(current_turret_position_low + low, current_turret_position_high + high);
}
void setup_turret()
{
  servo_turret_low.attach(12);
  servo_turret_high.attach(13);
  reset_turret_position(50);
}

//////////////
// IF Range Swiches
int setup_rangeswiches(){
  pinMode(switch_body_front_left, INPUT);
  pinMode(switch_body_front_right, INPUT);
  pinMode(switch_body_back_left, INPUT);
  pinMode(switch_body_back_right, INPUT);
}

//////////////
// LCD
void setup_lcd()
{
  lcd_body.init();
  lcd_body.backlight();
}
void write_lcd_message(const char* msg)
{
  write_lcd_message(0, 0, msg);
}
void write_lcd_message(const short l, const short c, const char* msg)
{
  lcd_body.clear();
  lcd_body.setCursor(l, c);
  lcd_body.print(msg);
}
void write_lcd_messages(const char* msg, const char* msg2)
{
  lcd_body.clear();
  lcd_body.setCursor(0, 0);
  lcd_body.print(msg);
  lcd_body.setCursor(0, 1);
  lcd_body.print(msg2);
}
void write_lcd_messages(const char* msg, const String msg2)
{
  lcd_body.clear();
  lcd_body.setCursor(0, 0);
  lcd_body.print(msg);
  lcd_body.setCursor(0, 1);
  lcd_body.print(msg2);
}
void write_lcd_buffer(const char *msg, const short len)
{
  lcd_body.print(msg);
}
void write_lcd_buffer(const short msg)
{
  char b[2] = {(char)msg, '\0'};
  lcd_body.print(b);
}


//////////////
// IR Range Sensors
short get_range_sensor_infra(const int infra, const int count=5, const int dlay=5)
{
  float fDistance = 0;

  for(int i=0; i<count; i++)
  {
    if(dlay>0) delay(dlay);
  
    float fScale = 5.0f/1024.0f;    // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    int iAnalogValue = analogRead(infra);
    float fVolts = iAnalogValue*fScale;

    // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
    fDistance += 65.0f*pow(fVolts, -1.10f); 
  }

  // retour en cm
  return (short)(fDistance/count);
}
short get_range_sensor_sonic(const int sonic, const int count=5, const int dlay=5)
{
  float distance = 0;

  for(int i=0; i<count; i++)
  {
    if(dlay>0) delay(dlay);

    long duration=0;
    pinMode(sonic, OUTPUT);
    digitalWrite(sonic, LOW);
    delayMicroseconds(2);
    digitalWrite(sonic, HIGH);
    delayMicroseconds(5);
    digitalWrite(sonic, LOW);

    pinMode(sonic, INPUT);
    duration = pulseIn(sonic, HIGH);
    
    distance += (float)(duration / 29 / 2);
  }
  
  // retour en cm
  return (short)(distance/count);
}

const int turret_ir_min = 35, turret_ir_max = 140;
const int turret_us_min = 5, turret_us_max = 500;
short get_range_turret(const int count=5, const int dlay=5, const boolean bUseSonic=true)
{
  // #tsl+35+115+1+75$
  short dist = get_range_sensor_infra(infra_range_turret, count, dlay);
  if(bUseSonic)
  {
    if(dist > turret_ir_max)
    {
      short dist2 =  get_range_sensor_sonic(sonic_range_turret, count, dlay);
      if(dist2>turret_ir_max)
      {
        if(dist<turret_us_max)  dist = dist2;
        else                    dist = turret_us_max;
      }
    }
    else if(dist<turret_ir_min)
    {
      short dist2 =  get_range_sensor_sonic(sonic_range_turret, count, dlay);
      if(dist2<turret_ir_min)
      {
        if(dist>turret_us_min)  dist = dist2;
        else                    dist = turret_us_min;
      }
    }
  }
  return dist;
}

const int body_ir_min = 35, body_ir_max = 140;
short get_range_body(const int count=5, const int dlay=5)
{
  short dist = get_range_sensor_infra(infra_range_body_front, count, dlay);
  return dist;
}
//////////////
// scan function !
void scan_point(const int iLow, const int iHigh)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iL = ibound(turret_low_min, turret_low_max, iLow);
  int iH = ibound(turret_high_min, turret_high_max, iHigh);

  write_lcd_messages("ScanPoint", "...");
  
  send_start_trame();
  Serial.print("scan_point:");
  Serial.print("data=");
  // set position
  set_turret_position(iL, iH);
  // get distance
  short iDist = get_range_turret();
  // write it
  Serial.print(iL);
  Serial.print(" ");
  Serial.print(iH);
  Serial.print(" ");
  Serial.print(iDist);    
  send_end_trame();

  set_turret_position(old_low, old_high);
  
  write_lcd_messages("ScanPoint", "ok");
}
void scan_line(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHigh)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = ibound(turret_low_min, turret_low_max, iLowBegin);
  int iLE = ibound(turret_low_min, turret_low_max, iLowEnd);
  int iH = ibound(turret_high_min, turret_high_max, iHigh);

  write_lcd_messages("ScanLine", "...");

  set_turret_position(iLB, iH);

  send_start_trame();
  Serial.print("scan_line:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high="); Serial.print(iH); Serial.print(",");
  Serial.print("data=");
  for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
  {
    // set position
    set_turret_position(iL, iH);
    // get distance
    short iDist = get_range_turret();

    // write it
    if(iL!=iLB)
      Serial.print(";");
    Serial.print(iL);
    Serial.print(" ");
    Serial.print(iH);
    Serial.print(" ");
    Serial.print(iDist);
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
  
  write_lcd_messages("ScanLine", "Ok");
}
// do a line and return to start fow new one
void scan_zone_z(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHighBegin, const int iHighEnd, const int iHighStep)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = ibound(turret_low_min, turret_low_max, iLowBegin);
  int iLE = ibound(turret_low_min, turret_low_max, iLowEnd);
  int iHB = ibound(turret_high_min, turret_high_max, iHighBegin);
  int iHE = ibound(turret_high_min, turret_high_max, iHighEnd);
  
  write_lcd_messages("ScanZone","...");

  set_turret_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high_start="); Serial.print(iHB); Serial.print(",");
  Serial.print("high_end="); Serial.print(iHE); Serial.print(",");
  Serial.print("high_step="); Serial.print(iHighStep); Serial.print(",");
  Serial.print("data=");

  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
    {
      // set position
      set_turret_position(iL, iH);
      
      // get distance
      short iDist = get_range_turret();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
  
   write_lcd_messages("ScanZone","ok");
}
// do interval pair/unpair... quicker but bug some where :(
void scan_zone_s(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHighBegin, const int iHighEnd, const int iHighStep)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = ibound(turret_low_min, turret_low_max, iLowBegin);
  int iLE = ibound(turret_low_min, turret_low_max, iLowEnd);
  int iHB = ibound(turret_high_min, turret_high_max, iHighBegin);
  int iHE = ibound(turret_high_min, turret_high_max, iHighEnd);
  
  write_lcd_messages("ScanZone S","...");
  
  set_turret_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); Serial.print(iLB); Serial.print(",");
  Serial.print("low_end="); Serial.print(iLE); Serial.print(",");
  Serial.print("low_step="); Serial.print(iLowStep); Serial.print(",");
  Serial.print("high_start="); Serial.print(iHB); Serial.print(",");
  Serial.print("high_end="); Serial.print(iHE); Serial.print(",");
  Serial.print("high_step="); Serial.print(iHighStep); Serial.print(",");
  Serial.print("data=");

  boolean leftoright = true;
  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    int lb, le, ls;
    
    if (leftoright)
    {
      lb = iLB;
      le = iLE+1;
      ls = iLowStep;
    }
    else
    {
      lb = iLE;
      le = iLB-1;
      ls = -iLowStep;
    }
    leftoright = !leftoright;
    
    for(int iL=lb; (!leftoright && iL<le) || (leftoright && iL>le); iL+=ls)
    {
      // set position
      set_turret_position(iL, iH);
      
      // get distance
      short iDist = get_range_turret();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
  
  write_lcd_messages("ScanZone S","ok");
}

//////////////////////////////
// trame
void send_start_trame(void)
{
  Serial.print("#");
}
void send_end_trame(void)
{
  Serial.println("$");
}

//////////////////////////////
// version
void send_version(void)
{
  send_start_trame();
  Serial.write("version:name=roby_bt,version=0.7");
  send_end_trame();
  write_lcd_messages("Version","Roby BT - 0.7");
}

//////////////////////////////
// setting
void send_setting(void)
{
  send_start_trame();
  Serial.print("setting:");
  Serial.print("send_status=");
  Serial.print(_send_status?"1":"0");
  send_end_trame();
  write_lcd_messages("Settings","sended");
}

void send_setting_body(void)
{
  send_start_trame();
  Serial.print("setting_body:");
  Serial.print("min_speed=0");
  Serial.print(",nice_speed=50");
  Serial.print(",max_speed=100");
  Serial.print(",min_dist=");
  Serial.print(body_ir_min);
  Serial.print(",max_dist=");
  Serial.print(body_ir_max);
  send_end_trame();
  write_lcd_messages("Settings Body", "sended");
}

void send_setting_turret(void)
{
  send_start_trame();  
  Serial.print("setting_turret:");
  Serial.print("min_low=");
  Serial.print(turret_low_min);
  Serial.print(",mid_low=");
  Serial.print(turret_low_mid);
  Serial.print(",max_low=");
  Serial.print(turret_low_max);
  Serial.print(",min_high=");
  Serial.print(turret_high_min);
  Serial.print(",mid_high=");
  Serial.print(turret_high_mid);
  Serial.print(",max_high=");
  Serial.print(turret_high_max);
  Serial.print(",min_dist=");
  Serial.print(turret_ir_min);
  Serial.print(",med_dist=");
  Serial.print(turret_ir_max);
  Serial.print(",max_dist=");
  Serial.print(turret_us_max);
  send_end_trame();
  write_lcd_messages("Settings Turret", "sended");
}

//////////////////////////////
// status
void send_status_sensors(void)
{
  boolean fl = (digitalRead(switch_body_front_left)==HIGH)?true:false;
  boolean fr = (digitalRead(switch_body_front_right)==HIGH)?true:false;
  boolean bl = (digitalRead(switch_body_back_left)==HIGH)?true:false;
  boolean br = (digitalRead(switch_body_back_right)==HIGH)?true:false;

  send_start_trame();
  Serial.print("sensors:");
  Serial.print("fl=");
  Serial.print(fl?"0":"1"); 
  Serial.print(",");
  Serial.print("fr="); 
  Serial.print(fr?"0":"1"); 
  Serial.print(",");
  Serial.print("bl="); 
  Serial.print(bl?"0":"1"); 
  Serial.print(",");
  Serial.print("br="); 
  Serial.print(br?"0":"1"); 
  send_end_trame();
}
void send_status_turret(void)
{
  short dist = get_range_turret();

  send_start_trame();
  Serial.print("turret:");
  Serial.print("low=");
  Serial.print(current_turret_position_low); 
  Serial.print(",");
  Serial.print("high="); 
  Serial.print(current_turret_position_high); 
  Serial.print(",");
  Serial.print("dist="); 
  Serial.print(dist);
  send_end_trame();
}
void send_status_body(void)
{
  get_body_compass();
  short dist = get_range_body();
  
  send_start_trame();
  Serial.print("body:");
  Serial.print("bear=");
  Serial.print(body_compass_bearing);
  Serial.print(",");
  Serial.print("pitch=");
  Serial.print(body_compass_pitch);
  Serial.print(",");
  Serial.print("roll=");
  Serial.print(body_compass_roll);
  Serial.print(",");
  Serial.print("dist=");
  Serial.print(dist);
  send_end_trame();
}
void get_body_compass(void)
{
   int iCompassI2C = 0x60;
   byte high, low, fine;              // highByte and lowByte store high and low bytes of the bearing and fine stores decimal place of bearing
   char pitch, roll;                          // Stores pitch and roll values of CMPS09, chars are used because they support signed value
   int bearing;                               // Stores full bearing
   
   Wire.beginTransmission(iCompassI2C);           //starts communication with CMPS09
   Wire.write(2);                              //Sends the register we wish to start reading from
   Wire.endTransmission();

   Wire.requestFrom(iCompassI2C, 4);              // Request 4 bytes from CMPS09
   while(Wire.available() < 4);               // Wait for bytes to become available
   high = Wire.read();           
   low = Wire.read();            
   pitch = Wire.read();              
   roll = Wire.read();               

   bearing = ((high<<8)+low);         // Calculate decimal place of bearing
  
   body_compass_bearing = bearing/10.f;
   body_compass_pitch = pitch;
   body_compass_roll = roll;
}

//////////////////////////////
// serial buffer loading
const int _iMaxSerialBuffer = 512;
int _iSerialBufferStart=0, _iSerialBufferLen=0, _iSerialBufferIndex=0;
char _SerialBuffer[_iMaxSerialBuffer];
boolean readSerialString (String &cmd) {
  int sb;
  while (Serial.available())
  {
    sb = Serial.read();
    if(_enable_debug) write_lcd_buffer(sb);
    if(sb=='#') // start cmd ?
    {
      _iSerialBufferStart = _iSerialBufferIndex+1;
    }
    else if(sb=='$' && _iSerialBufferStart>=0) // end cmd ?
    {
      _iSerialBufferLen = _iSerialBufferIndex;
      _SerialBuffer[_iSerialBufferLen] = '\0';
      cmd = &_SerialBuffer[_iSerialBufferStart];
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return true;
    }
    if(_iSerialBufferIndex>=_iMaxSerialBuffer)
    {
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return false;
    }
    _SerialBuffer[_iSerialBufferIndex] = sb;
    _iSerialBufferIndex++;
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, const int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

void check_commands(void)
{
  String line="";
  if(!readSerialString(line))
    return;
  if(line.length()>0)
  {
    // debug only : Serial.println("cmd : "+line);

    switch(line[0])
    {
    case 'v': // (V)ersion
      {
        // #v$
        send_version();
      }
      break;      
    case 'g': // settin(G)
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 'b':  // (B)ody setting
              // #gab$
              send_setting_body();        
              break;
            case 't':  // (T)urrel setting
              // #gat$
              send_setting_turret();
              break;
            default: // general setting
              // #ga-$
              send_setting();
              break;
            }
          }
          break;
        }
      } 
      break;            
    case 's': // (S)tatus    
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 's':  // (S)ensors
              // #sas$
              send_status_sensors();        
              break;
            case 't':  // (T)urrel
              // #sat$
              send_status_turret();
              break;
            case 'b':  // (B)ody
              // #sab$
              send_status_body();
              break;
            }
          }
          break;
        case 's': // (S)et
          {
            switch(line[2])
            {
            case 's': // (S)end status automatic
              {
                // #sss0$/#sss1$
                char param = line[3];
                if(param=='0') _send_status = false;
                else if(param=='1') _send_status = true;
                send_setting();
              }
              break;
            }
          }
          break;
        }
      }
      break;
    case 'r': // (R)eset
      {
        switch(line[1])
        {
        case 'b': // (B)ody
          // #rb$
          motor_stop();
          break;
        case 't': // (T) turret
          // #rt$
          reset_turret_position();
          if(!_send_status) send_status_turret();
          break;
        default: // All !
          // #r-$
          motor_stop(); 
          reset_turret_position();
          if(!_send_status) send_status_turret();
          break;
        }
      }
      break;      
    case 'b': // (B)ody stuff
      {
        switch(line[1])
        {
        case 'm': // 'M'oving
          {
            // #bm11+25+25$
            boolean bL = (line[2]=='0')?false:true;
            boolean bR = (line[3]=='0')?false:true;
            int iL=0, iR=0;
            int endl = load_int(line, 4, iL);
            load_int(line, endl, iR);
            int iPwnL = map(iL, 0, 100, 50, 250);
            int iPwnR = map(iR, 0, 100, 50, 250);
            motor_action(iPwnL, iPwnR, bL, bR);
          }
        }
      }
      break;      
    case 't': // (T)urrel stuff
      {
        switch(line[1])
        {
        case 'p': // set 'P'osition
          {
            // #tp+90+90$
            int iLow=0, iHigh=0;
            int endl = load_int(line, 2, iLow);
            load_int(line, endl, iHigh);
            set_turret_position(iLow, iHigh);
            if(!_send_status) send_status_turret();
          }
          break;
        case 's': // 'S'can mode !
          {
            switch(line[2])
            {
            case 'p':
              {
                // #tsp+90+90$
                int iLow=0, iHigh=0;
                int endl = load_int(line, 3, iLow);
                load_int(line, endl, iHigh);
                scan_point(iLow, iHigh);
                if(!_send_status) send_status_turret();
              }
              break;
            case 'l':
              {
                // #tsl+85+95+1+75$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHigh=0;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                load_int(line, endl, iHigh);
                scan_line(iLowBegin, iLowEnd, iLowStep, iHigh);
                if(!_send_status) send_status_turret();
              }
              break;
            case 'z':
              {
                // #tsz+85+95+1+85+95+1$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHighBegin=0, iHighEnd=0, iHighStep;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                endl = load_int(line, endl, iHighBegin);
                endl = load_int(line, endl, iHighEnd);                
                load_int(line, endl, iHighStep);
                scan_zone_z(iLowBegin, iLowEnd, iLowStep, iHighBegin, iHighEnd, iHighStep);
                if(!_send_status) send_status_turret();
              }
              break;
            }
          }
          break;
        }
      }
      break;      
    default:
      if(_send_invalid_cmd)
      {
        send_start_trame();
        Serial.print("error:invalid cmd '");
        Serial.print(line+"'");
        send_end_trame();
        
        write_lcd_messages("invalid cmd",line);
      }
      break;
    }
  }
}

void check_hitsensors(void)
{
  boolean fl = (digitalRead(switch_body_front_left)==HIGH)?true:false;
  boolean fr = (digitalRead(switch_body_front_right)==HIGH)?true:false;
  boolean bl = (digitalRead(switch_body_back_left)==HIGH)?true:false;
  boolean br = (digitalRead(switch_body_back_right)==HIGH)?true:false;
  if(fl || fr || bl || br)
  {
    char s[5] = "....";
    if(fl) s[0]='!';
    if(fr) s[1]='!';
    if(bl) s[2]='!';
    if(br) s[3]='!';
    write_lcd_messages("Body Hit Detected", s);
    send_status_sensors();
  }
}

//////////////
// setup
void setup(void)
{
  Serial.begin(9600); // Set Baud Rate
  Wire.begin();  
  setup_lcd();
  write_lcd_messages("Setup","Init...");
  setup_motor();
  setup_rangeswiches();
  setup_turret();
  delay(500);  
  write_lcd_messages("Setup","Ok");
}

///////////////////////////////
// loop
void loop(void)
{
  check_commands();

  if(_send_status)
  {
    unsigned long _rapport_last = millis();
    if(( _rapport_last - _rapport_turret_last) > (_setting_rapport_turret_interval*_setting_rapport_stepping))
    {
      send_status_turret();
      _rapport_turret_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_body_last) > (_setting_rapport_body_interval*_setting_rapport_stepping))
    {
      send_status_body();
      _rapport_body_last =  _rapport_last;
    }
    if(( _rapport_last - _rapport_sensors_last) > (_setting_rapport_sensors_interval*_setting_rapport_stepping))
    {
      send_status_sensors();
      _rapport_sensors_last =  _rapport_last;
    }
  }
  else
  {
     // check_hitsensors(); //bug ?
  }
}

























